package com.roobo.hardware;

import java.io.File;
import java.io.FileOutputStream;
import java.io.IOException;
import java.io.OutputStreamWriter;
import java.io.Writer;

import android.content.BroadcastReceiver;
import android.content.Context;
import android.content.Intent;

import android.util.Log;

/*
 * Created by weidu on 16-11-10.
 * <p/>
 * 20170314,we use per client-monitor at server.so
 * we changed the route here:
 * 1. call setMotionStateChangeListener and connect at internalInit;
 * 2. clear moto-monitor when disconnect.
 */
public class MotionCtrlManager {

    private static final String TAG = "MotionCtrlManager";

    public static final int MOTO_STATE_IDLE = 0;
    public static final int MOTO_STATE_TURNING_LEFT = 1;
    public static final int MOTO_STATE_TURNING_RIGHT = 2;
    public static final int MOTO_STATE_STOP = 3;
    public static final int MOTO_STATE_CALIBRATING = 4;
    public static final int MOTO_STATE_CALIB_DONE = 5;
    public static final int MOTO_STATE_CHECKMAX_DONE  = 6; // 20170425,add for check max left/right degree!

    public static final String[] STATE = {"IDLE", "TURNING_LEFT", "TURNING_RIGHT", "STOP", "CALIBRATING", "CALIB_DONE"};

    public enum MotorState {
        idle("IDLE", 0),
        turningLeft("TURNING_LEFT", 1),
        turningRight("TURNING_RIGHT", 2),
        stop("STOP", 3),
        calibrating("CALIBRATING", 4),
        calibDone("CALIB_DONE", 5),
        checkMaxDone("CHECKMAX_DONE", 6),
        ;
        private final int state;
        private final String stateName;
        private MotorState(String name, int state) {
            // TODO Auto-generated constructor stub
            this.state = state;
            this.stateName = name;
        }

        public int getState() {
            return state;
        }

        public String getStateName() {
            return stateName;
        }
    }

    private MotorState currentMotorState = MotorState.idle;

    // 20170427,the max allow degree error.
    private static final int MOTO_MAX_DEGREE_ERROR = 10;
    private static MotionCtrlManager sInstance = null;

    private MotionExecuteCallback mMotionExecuteCallback = null;
    private RobotMotionStateListener mMotionStateListener = null;

    private boolean mConnected = false;
    private int mCurState = MOTO_STATE_IDLE;

    private MotionCtrlManager() {
    }

    public static synchronized MotionCtrlManager getInstance() {
        if (sInstance == null) {
            sInstance = new MotionCtrlManager();
        }
        return sInstance;
    }

    public void setRobotMotionStateListener(RobotMotionStateListener listener) {
        mMotionStateListener = listener;
    }

    public void setMotionExecuteCallback(MotionExecuteCallback callback) {
        if (mMotionExecuteCallback != null) {
            mMotionExecuteCallback.onLostFocus();
        }
        mMotionExecuteCallback = callback;
    }

    /*
     * Connect to moto server,must call this method before run motion.
     */
    public void connect() {
    }

    /*
     * Disconnect with moto server,must call this method after all motion completed.
     */
    public void disconnect() {
    }

    /*
     * 获得向右旋转的最大度数
     * 20170303，这个左右可以选择的角度，通过校准来得到，有可能
     * 左右角度不是一样的，需要通过下面两个接口来获取。
     */
    public int getMaxRightAngle() {
        return 180;
    }

    /*
     * 获得向左旋转的最大度数
     */
    public int getMaxLeftAngle() {
        return 180;
    }

    /*
     * 判断当前角度是否到达了左边(<0)或者右边(>0)的最大角度。
     * return : true -- 到达左边或者右边的最大角度。
     *          false -- 未到达最大角度。
     */
    public boolean isDegreeEnd(int curDegree) {
        int maxDegree;
        if( curDegree > 0 ){
            maxDegree = getMaxRightAngle();
        } else {
            maxDegree = getMaxLeftAngle();
            curDegree = -curDegree;
        }
        if( Math.abs(maxDegree - curDegree) <= MOTO_MAX_DEGREE_ERROR ){
            return true;
        }
        return false;

    }
    /*
     * 获取舵机当前的绝对位置。
     *
     * @return <br>
     * < 0 ,表示 左转的角度，从 -getMaxLeftAngle .. 0
     * =0  ,表示在中间位置。
     * > 0 ,表示右转的角度，从 0 .. getMaxRightAngle.
     */
    public int getCurrentPosition() {
        return 0;
    }

    /*
     * @return MOTO_STATE_IDLE 或者 MOTO_STATE_TURNING_LEFT 或者 MOTO_STATE_TURNING_RIGH
     */
    public int getMotionState() {
        Log.d(TAG, "getMotionState");
        return currentMotorState.getState();
    }

    /*
     * @return -- true: the moto already calibrated.
     */
    public boolean getCalibrationState() {
        return true;
    }

    /*
     * start calibration. the calibration will make the
     * MOTO turn to left-max degree,and then right-max
     * degree,and record the adc-value for checking real degree.
     */
    public void startCalibration() {
    }

    /*
     * 停止舵机转动，舵机停在当前位置。如果舵机已经停止，调用函数无作用。
     */
    public void stop() {
        configMotor(0, 0, MotorState.stop);
    }

    /*
     * 复位让舵机转到中间 0 度位置。然后停止。复位转速。
     * <p/>
     * 20161224,add new parameter:
     * msPerDegree == -1: stop first(this will make calibration stop).
     * msPerDegree == -2: open debug,set kernel check_stuck_not_stop_moto = true;
     * msPerDegree == -3: close debug.
     */
    public void reset(int msPerDegree) {
        Log.d(TAG, "reset msPerDegree=" + msPerDegree);
    }

    /*
     * Turn to left with a absolutely degree.
     *
     * @param angle
     * @param msPerDegree
     */
    public void turnLeft(int angle, int msPerDegree) {
        Log.d(TAG, "turnLeft,angle=" + angle + " msPerDegree=" + msPerDegree);
        configMotor(msPerDegree, angle, MotorState.turningLeft);
    }

    /*
     * Turn to right with a absolutely degree.
     *
     * @param angle
     * @param msPerDegree
     */
    public void turnRight(int angle, int msPerDegree) {
        Log.d(TAG, "turnRight,angle=" + angle + " msPerDegree=" + msPerDegree);
        configMotor(msPerDegree, angle, MotorState.turningRight);
    }

    /*
     * 相对当前的角度向左转的度数
     * angle:
     * 向左转的相对角度。最总角度是在当前角度的基础上左转。
     * 超出系统最大的左转角度部分被忽略。
     * <p/>
     * msPerDegree:
     * 每转一度的延迟，可以控制舵机的转速。范围 0..500 ms.
     * 0 表示全速转动。
     */
    public void turnLeftRelative(int angle, int msPerDegree) {
        Log.d(TAG, "turnLeftRelative,angle=" + angle + " msPerDegree=" + msPerDegree);
        configMotor(msPerDegree, angle, MotorState.turningLeft);
    }

    /*
     * 相对当前角度向右转的度数
     * angle:
     * 向右转的相对角度。超出右转最大角度的部分被忽略。
     * msPerDegree:
     * 每转一度的延迟，可以控制舵机的转速。范围 40..500 ms.
     * 0 表示全速转动。
     */
    public void turnRightRelative(int angle, int msPerDegree) {
        Log.d(TAG, "turnRightRelative,angle=" + angle + " msPerDegree=" + msPerDegree);
        configMotor(msPerDegree, angle, MotorState.turningRight);
    }

    /*
     * 左转到头
     * 从当前位置（任何位置）左转到头，即转到  getMaxLeftAngle
     * msPerDegree：
     * 控制转速，参数和 turnRight 说明一样。20170222,don't use this function .
     * <p/>
     * when msPerDegree = 0XFFFF -- start ps calib.
     * msPerDegree = 0XFFFE -- stop ps calib.
     */
    public void turnLeftToEnd(int msPerDegree) {
        Log.d(TAG, "turnLeftToEnd=" + msPerDegree);
        configMotor(msPerDegree, getMaxLeftAngle(), MotorState.turningRight);
    }

    /*
     * 右转到头
     * 从当前位置（任何位置）右转到头，即转到  getMaxRightAngle
     * 20170222,don't use this function .
     */
    public void turnRightToEnd(int msPerDegree) {
        Log.d(TAG, "turnRightToEnd msPerDegree=" + msPerDegree);
        configMotor(msPerDegree, getMaxRightAngle(), MotorState.turningRight);
    }

    /*
     * turn relative degree with callback.
     *
     * @param angle       < 0 : turn left -angle;
     *                    > 0 : turn right angle;
     * @param msPerDegree -- from 0 -- 90 ms.
     */
    public void turn(int angle, int msPerDegree) {
        if (angle <= 0) {
            turnLeftRelative(angle, msPerDegree);
        } else {
            turnRightRelative(angle, msPerDegree);
        }
    }

    /*
     * turn absolutely degree with callback.
     *
     * @param angle       < 0 : turn to left -angle;
     *                    > 0 : turn to right angle;
     * @param msPerDegree
     */
    public void turnAbs(int angle, int msPerDegree) {
        if (angle <= 0) {
            turnLeft(angle, msPerDegree);
        } else {
            turnRight(angle, msPerDegree);
        }
    }

    private void checkConnectionState() {

    }

    /*
     * 1. 转动到位 2. 转到边界 3. 转到边界后 接续接收到运动指令 4. 未转动到位 5.校准中 6. 未校准
     *
     * @param state
     * @param position
     */
    private void onStateChange(int state, int position) {
        MotionExecuteCallback callback = mMotionExecuteCallback;
        switch (state) {
            case MOTO_STATE_IDLE:
                if (mCurState == MOTO_STATE_IDLE) {
                    if (callback != null) {
                        callback.onMotionStop();
                    }
                } else {
                    if (callback != null) {
                        callback.onMotionComplete();
                    }
                }
                mCurState = MOTO_STATE_IDLE;
                break;
            case MOTO_STATE_STOP:
                if (mCurState == MOTO_STATE_IDLE) {
                    if (callback != null) {
                        callback.onMotionError("Error State Stop");
                    }
                } else {
                    if (callback != null) {
                        callback.onMotionStop();
                    }
                }
                mCurState = MOTO_STATE_IDLE;
                break;
            case MOTO_STATE_TURNING_LEFT:
                if (mCurState == MOTO_STATE_IDLE) {
                    Log.d(TAG, "Got TurnLeft at Idle");
                    if (callback != null) {
                        callback.onMotionStart();
                    }
                } else {
                    if (mCurState != MOTO_STATE_TURNING_LEFT) {
                        Log.d(TAG, "Got TurnLeft at " + getState(mCurState));
                        if (callback != null) {
                            callback.onMotionError("Error State TurnLeft-" + mCurState);
                        }
                    }
                }
                mCurState = state;
                break;
            case MOTO_STATE_TURNING_RIGHT:
                if (mCurState == MOTO_STATE_IDLE) {
                    Log.d(TAG, "Got TurnRight at Idle");
                    if (callback != null) {
                        callback.onMotionStart();
                    }
                } else {
                    if (mCurState != MOTO_STATE_TURNING_RIGHT) {
                        Log.d(TAG, "Got TurnRight at " + getState(mCurState));
                        if (callback != null) {
                            callback.onMotionError("Error State TurnRight-" + getState(mCurState));
                        }
                    }
                }
                mCurState = state;
                break;
            case MOTO_STATE_CALIBRATING:
                mCurState = MOTO_STATE_CALIBRATING;
                break;
            case MOTO_STATE_CALIB_DONE:
                mCurState = MOTO_STATE_IDLE;
                break;
            case MOTO_STATE_CHECKMAX_DONE:
                mCurState = MOTO_STATE_IDLE;
                break;
        }
    }

    private String getState(int state) {

        for (MotorState sta : MotorState.values()) {
            if (sta.getState() == state) {
                return sta.getStateName();
            }
        }
        return "UNKNOWN";
    }

    public interface MotionExecuteCallback {
        /*
         * the moto begin to start.
         */
        void onMotionStart();

        /*
         * the moto request is successful complete.
         */
        void onMotionComplete();

        /*
         * something wrong happened.
         *
         * @param errorMsg -- the wrong message.
         */
        void onMotionError(String errorMsg);

        /*
         * the request is done,but NOT finish.example: we want to turn to 120 degree,
         * but the moto stop at 100 degree.
         */
        void onMotionStop();

        /*
         * notify current motion callback when another scene set motion callback
         * and obtain the motion focus.
         */
        void onLostFocus();
    }

    public interface RobotMotionStateListener {
        /*
         * call when moto state or degree changed.
         * state:@see MOTO_STATE_XXXX
         */
        void onStateChange(int state, int position);
    }


    private final static String MOTOR_ACTION = "android.intent.action.MOTOR_SWITCH";
    private final String MOTOR_PATH = "/sys/class/pwm_motor/pwm";
    private static final int BIT_LEFT_IR = (1 << 0);
    private static final int BIT_RIGHT_IR = (1 << 1);
    private static final int BIT_MOTOR_FAULT = (1 << 2);

    private BroadcastReceiver motorReceiver = new BroadcastReceiver() {
        @Override
        public void onReceive(Context context, Intent intent) {
            final String action = intent.getAction();
            if (action.equals(MOTOR_ACTION)) {
                int state = intent.getIntExtra("state", 0);
                String name = intent.getStringExtra("name");

                StringBuilder sb = new StringBuilder("state = " + state);
                Log.e(TAG, "name = " + name + ", state = " + state);
                if (0 != (state & BIT_LEFT_IR)) {
                    sb.append(", IR_LEFT");
                }
                if (0 != (state & BIT_RIGHT_IR)) {
                    sb.append(", IR_RIGHT");
                }
                if (0 != (state & BIT_MOTOR_FAULT)) {
                    sb.append(", MOTOR_FAULT");
                }
            }
        }
    };


    /*
     *
     * @param msPerDegress from 0 -- 90 ms.<p>
     *                      period 越大， 转速越慢。 period 范围 [100, 1000]<p>
     *                      period = 10 * msPerDegress + 100
     * @return
     */
    private int msPerDegree_to_period(int msPerDegress) {
        return 10 * msPerDegress + 100;
    }

    private void configMotor(int msPerDegree, int degree, MotorState motorState) {
        int state = 3;//stop
        boolean antiClock = true;

        currentMotorState = motorState;

        switch (motorState) {
        case turningLeft:
            state = 6;
            break;
        case turningRight:
            state = 6;
            antiClock = false;
            break;

        default:
            break;
        }
        writeMotor(msPerDegree_to_period(msPerDegree), degree, state, antiClock);
    }

    //write period,degree,state,direction > /sys/class/pwm_motor/pwm
    //state: 3,stop, 6,move
    /*
     *
     * @param period for PWM;
     * @param degree for distance;
     * @param state
     * @param directionAnticlock true:anticlock; false:clock<p>
     *  内核  MOTOR_CTRL_DIR_CLOCK = 0x01; MOTOR_CTRL_DIR_ANTICLOCK = 0x02.
     *
     */
    private void writeMotor(int period, int degree, int state, boolean directionAnticlock) {

        String content = period + "," + degree + "," + state + "," + (directionAnticlock ? 0 : 1);

        writeStringToFile(MOTOR_PATH, content);

    }

    private synchronized static void writeStringToFile(String filePath, String content) {
        File file = new File(filePath);
        Writer writer = null;
        try {
            writer = new OutputStreamWriter(new FileOutputStream(file));
            writer.write(content,  0, content.length());
            writer.flush();
        } catch (IOException e) {
            e.printStackTrace();
        } finally {
            if (writer != null) {
                try {
                    writer.close();
                    Log.e(TAG, "writer close");
                } catch (IOException e1) {
                    e1.printStackTrace();
                }
            }
        }
    }
}
